
#pragma warning (disable : 4251 4996 4819 4244)
#include <new>
#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <ctime>
#include "ompl/util/RandomNumbers.h"
#include <boost/shared_array.hpp>
#include <boost/random.hpp>
#include <libplayerc++/playerc++.h>
#include "../source/StateRecorder.h"
#include "VtExecutor.h"
#include "../source/PlayerObject.h"
#include "../source/PlayerObjectManager.h"
#include "../source/pause.h"
#include "../source/PlayerProxy.h"

using namespace PlayerCc;
using namespace ompl::base;
using namespace ompl::geometric;


static const double MAX_PLAN_TIME = 0.01;

class Controller {

  ompl::RNG rng;

  // player parameters
  PlayerClient&    robot;
  SimulationProxy sim;
  MapProxy&        map;
  Position2dProxy pp;
  Position2dProxy gocon;
  Graphics2dProxy g;

  double reachDist;

public:

  Controller() :
    robot(PlayerProxy::Instance().GetClient()),
    sim(&robot, 0),
    map(PlayerProxy::Instance().GetMap()),
    pp(&robot, 0),
    gocon(&robot, 100),
    g(&robot, 0),
    reachDist(0.5)
  {
    robot.Read();
  }

private:
  // generates a valid goal position

public:

  // control loop
  void run() {

      bool bBack = false;
    for (;;) {
        // generate a list of orders to fulfil
        Position s0 = { 0.0, 2.0, 0.0 };
        Position s1 = { 1.0, 2.0, 0.0 };
        Position s2 = { 2.0, 2.0, 0.0 };
        Position s25 = { 2.5, 2.0, 0.0 };
        Position s3 = { 3.0, 2.0, 0.0 };
        Position s35 = { 3.5, 2.0, 0.0 };
        Position s4 = { 4.0, 2.0, 0.0 };
        Position s45 = { 4.5, 2.0, 0.0 };
        Position s5 = { 5.0, 2.0, 0.0 };
        Position s6 = { 6.0, 2.0, 0.0 };
        Position s_1 = { -1.0, 2.0, 0.0 };
        if ( bBack )
        {
            std::vector<Position> vecDest;
//            vecDest.push_back(s6);
//            vecDest.push_back(s5);
            vecDest.push_back(s4);
            vecDest.push_back(s3);
            vecDest.push_back(s2);
//            vecDest.push_back(s1);
//            vecDest.push_back(s0);
//            vecDest.push_back(s_1);
//            Position stDest = { 1.0, 2.0, 0.0 };
//            vecDest.push_back(stDest);
            Executor executor(vecDest, robot, pp, gocon);
            executor.execute();
        }
        else
        {
            std::vector<Position> vecDest;

//            vecDest.push_back(s_1);
//            vecDest.push_back(s0);
//            vecDest.push_back(s1);
            vecDest.push_back(s2);
            vecDest.push_back(s25);
            vecDest.push_back(s3);
            vecDest.push_back(s35);
            vecDest.push_back(s4);
            vecDest.push_back(s45);
//            vecDest.push_back(s5);
//            vecDest.push_back(s6);
            Position stDest = { 5.0, 2.0, 0.0 };
            vecDest.push_back(stDest);
            Executor executor(vecDest, robot, pp, gocon);
            executor.execute();
        }

        bBack = !bBack;
        
        util::pause( 1) ;
      }

  }
};

int
main(int argc, char *argv[])
{
    std::vector<std::string> vecDefFiles;
    vecDefFiles.push_back( "../worlds/tables.inc" );
    vecDefFiles.push_back( "../worlds/person.inc" );
    PlayerObjectManager::Instance().ParseConfigFile( vecDefFiles, "../worlds/restaurant4.world" );
  try {
    Controller control;
    control.run();
    return 0;
  } catch (std::exception &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}
